#include "pid_controller.hpp"

PIDController::PIDController(const PIDParams& params) 
    : params_(params), previous_error_(0.0f), integral_(0.0f), last_output_(0.0f) {
}

float PIDController::calculate(float current, float target, float dt) {
    if (std::isnan(current) || std::isnan(target) || std::isnan(dt) || dt <= 0) {
        return last_output_;
    }

    float error = target - current;

    // Integral term with windup protection
    integral_ += error * dt;
    integral_ = std::clamp(integral_, -params_.integral_limit, params_.integral_limit);

    // Derivative term
    float derivative = (error - previous_error_) / dt;
    previous_error_ = error;

    // PID output with limits
    float output = params_.kp * error + params_.ki * integral_ + params_.kd * derivative;
    output = std::clamp(output, params_.output_limit_min, params_.output_limit_max);

    last_output_ = output;
    return output;
}

void PIDController::reset() {
    previous_error_ = 0.0f;
    integral_ = 0.0f;
    last_output_ = 0.0f;
}

void PIDController::updateParams(const PIDParams& params) {
    params_ = params;
}